//
// Created by LinorInk on 2024/5/13.
//
#include "user.h"
#include "task.h"
#include "dr16_inf.h"
#include "servo_inf.h"
#include "dr16.h"

#define DR16_CONTROLLER_SENSOR 500

static RC_ctrl_t* rc_ctrl;
void dr16_controller_task();

taskTypedef DR16_controller_task_t;

void dr16_controller_init(void) {
    DR16_COMMAND = IDLE;
    rc_ctrl = get_RC_ctrl_point();

    Task_regist(&DR16_controller_task_t, 0, 0xff, dr16_controller_task);
}

void dr16_controller_task() {
    if(rc_ctrl->rc.switch_left == RC_SW_UP){
        /*
        慢（逆）  
            0,10

        慢（顺）
            50

        快（顺）  
            135,150

        停  
            270,260,250,240,360
            20,30
            -135,-50

        */

        /*

        开 
            全开 
                135 270 100
                80 开98%
            50 开将近80%
        
        关 
            全关
                0
        */


        //servo_setAngle(18,270);
        servo_setAngle(18,10);//关

        // static uint16_t angle_1 = 0;
        // servo_setAngle(18,angle_1);
        // angle_1 += 10;
        // if(angle_1 > 270) angle_1 = 0;
    }
    else if (rc_ctrl->rc.switch_left == RC_SW_MID) {
        //servo_setAngle(18, 135);
        servo_setAngle(18,80);//开

        // static uint16_t angle_2 = 270;
        // servo_setAngle(18, angle_2);
        // angle_2 -= 10;
        // if (angle_2 < 10) angle_2 = 270;

    }else if (rc_ctrl->rc.switch_left == RC_SW_DOWN) {
        //servo_setAngle(18, 135);
        //servo_setAngle(18,135);

        // static uint16_t angle_2 = 270;
        // servo_setAngle(18, angle_2);
        // angle_2 -= 10;
        // if (angle_2 < 10) angle_2 = 270;

    }

    if(rc_ctrl->rc.switch_right == RC_SW_UP){
        DR16_COMMAND = IDLE_HIGH;
        return;
    }else if(rc_ctrl->rc.switch_right == RC_SW_DOWN){
        DR16_COMMAND = IDLE_SHORT;
        return;
    }

    // if(rc_ctrl->rc.switch_left != 2){


    if(rc_ctrl->rc.rocker_l1 > DR16_CONTROLLER_SENSOR) {
        DR16_COMMAND = MOVE_FORWARD;
        return;
    }
    else if(rc_ctrl->rc.rocker_l1 < -DR16_CONTROLLER_SENSOR) {
        DR16_COMMAND = MOVE_BACKWARD;
        return;
    }

    if(rc_ctrl->rc.rocker_l_ > DR16_CONTROLLER_SENSOR) {
        DR16_COMMAND = MOVE_RIGHT;
        return;
    }
    else if(rc_ctrl->rc.rocker_l_ < -DR16_CONTROLLER_SENSOR) {
        DR16_COMMAND = MOVE_LEFT;
        return;
    }

    if(rc_ctrl->rc.rocker_r_ > DR16_CONTROLLER_SENSOR) {
        DR16_COMMAND = TURN_ROUND_LEFT;
        return;
    }
    else if(rc_ctrl->rc.rocker_r_ < -DR16_CONTROLLER_SENSOR) {
        DR16_COMMAND = TURN_ROUND_RIGHT;
        return;
    }

    if(rc_ctrl->rc.dial > DR16_CONTROLLER_SENSOR){
        DR16_COMMAND = TURN_ROUND_LEFT_HIGH;
        return;
    }
    else if(rc_ctrl->rc.dial < -DR16_CONTROLLER_SENSOR){
        DR16_COMMAND = TURN_ROUND_RIGHT_HIGH;
        return;
    }

    if(rc_ctrl->rc.rocker_r1 > DR16_CONTROLLER_SENSOR){
        DR16_COMMAND = IDLE_DOWN;
        return;
    }else if(rc_ctrl->rc.rocker_r1 < -DR16_CONTROLLER_SENSOR){
        DR16_COMMAND = CHEER_1;
        return;
    }

    // }

    // if(rc_ctrl->rc.switch_left == 2){
    //     if(rc_ctrl->rc.rocker_l1 > DR16_CONTROLLER_SENSOR) {
    //         DR16_COMMAND = test;
    //         return;
    //     }
    // }
    

    

    DR16_COMMAND = IDLE;
}
